#include "rclcpp/rclcpp.hpp"
#include "nav_msgs/msg/odometry.hpp"

class OdometryRepub : public rclcpp::Node
{
public:
    OdometryRepub()
        : Node("odometry_repub")
    {
        // 订阅
        sub1_ = this->create_subscription<nav_msgs::msg::Odometry>(
            "odom_transformed", 10,
            std::bind(&OdometryRepub::odom_callback1, this, std::placeholders::_1));

        sub2_ = this->create_subscription<nav_msgs::msg::Odometry>(
            "odometry/gps", 10,
            std::bind(&OdometryRepub::odom_callback2, this, std::placeholders::_1));

        // 发布
        pub1_ = this->create_publisher<nav_msgs::msg::Odometry>("odom_transformed_repub", 10);
        pub2_ = this->create_publisher<nav_msgs::msg::Odometry>("odometry/gps_repub", 10);

        RCLCPP_INFO(this->get_logger(), "重转发节点已启动");
    }

private:
    void odom_callback1(const nav_msgs::msg::Odometry::SharedPtr msg)
    {
        auto msg_new = *msg;
        msg_new.pose.pose.position.z = 0.0;
        msg_new.twist.twist.linear.z = 0.0;
        // 可按需保留angular.z
        pub1_->publish(msg_new);
        RCLCPP_INFO(this->get_logger(),
                    "转换后的里程计数据已发布: [%.3f, %.3f, %.3f]",
                    msg_new.pose.pose.position.x,
                    msg_new.pose.pose.position.y,
                    msg_new.pose.pose.position.z);
    }

    void odom_callback2(const nav_msgs::msg::Odometry::SharedPtr msg)
    {
        auto msg_new = *msg;
        msg_new.pose.pose.position.z = 0.0;
        msg_new.twist.twist.linear.z = 0.0;
        pub2_->publish(msg_new);
        RCLCPP_INFO(this->get_logger(),
                    "转换后的GPS里程计数据已发布: [%.3f, %.3f, %.3f]",
                    msg_new.pose.pose.position.x,
                    msg_new.pose.pose.position.y,
                    msg_new.pose.pose.position.z);
    }

    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub1_;
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub2_;
    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub1_;
    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub2_;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<OdometryRepub>());
    rclcpp::shutdown();
    return 0;
}